#include "GlutViewer.h"

#include "Rotation.h"
#include "Platform.h"

using namespace std;
using namespace alvar;
using namespace GlutViewer;

Drawable::Drawable(double _scale, double _r, double _g, double _b)
{
  SetScale(_scale);
  SetColor(_r, _g, _b);
}

void Drawable::SetScale(double _scale)
{
  scale = _scale;
}

void Drawable::SetColor(double _r, double _g, double _b)
{
  color[0] = _r;
  color[1] = _g;
  color[2] = _b;
}

void Drawable::Draw()
{
  // double color[3] = {1, 1, 1};
  glPushMatrix();
  glMultMatrixd(gl_mat);
  DrawAxis(scale, color);
  glPopMatrix();
}

void Drawable::DrawAxis(double scale, double color[3])
{
  glEnable(GL_DEPTH_TEST);
  glDisable(GL_CULL_FACE);

  glColor3d(color[0], color[1], color[2]);
  glBegin(GL_QUADS);
  glVertex3f(-scale / 2, -scale / 2, 0.0);
  glVertex3f(-scale / 2, scale / 2, 0.0);

  glVertex3f(-scale / 2, scale / 2, 0.0);
  glVertex3f(scale / 2, scale / 2, 0.0);

  glVertex3f(scale / 2, scale / 2, 0.0);
  glVertex3f(scale / 2, -scale / 2, 0.0);

  glVertex3f(scale / 2, -scale / 2, 0.0);
  glVertex3f(-scale / 2, -scale / 2, 0.0);
  glEnd();

  // Z
  glColor3d(0.0, 0.0, 1.0);
  glBegin(GL_LINES);
  glVertex3f(0.0, 0.0, 0.0);
  glVertex3f(0.0, 0.0, scale);
  glEnd();

  glDisable(GL_DEPTH_TEST);

  // X
  glColor3d(1.0, 0.0, 0.0);
  glBegin(GL_LINES);
  glVertex3f(0.0, 0.0, 0.0);
  glVertex3f(scale, 0.0, 0.0);
  glEnd();

  // Y
  glColor3d(0.0, 1.0, 0.0);
  glBegin(GL_LINES);
  glVertex3f(0.0, 0.0, 0.0);
  glVertex3f(0.0, scale, 0.0);
  glEnd();
}

void Drawable::SetGLMatTraQuat(double* tra, double* quat, bool flip)
{
  Rotation r;
  if (quat != 0)
  {
    CvMat cv_mat;
    cvInitMatHeader(&cv_mat, 4, 1, CV_64F, quat);
    r.SetQuaternion(&cv_mat);
  }

  int flp = 1;
  if (flip)
  {
    r.Transpose();
    // flp=-1;
  }

  CvMat cv_gl_mat;
  cvInitMatHeader(&cv_gl_mat, 4, 4, CV_64F, gl_mat);
  cvZero(&cv_gl_mat);
  r.GetMatrix(&cv_gl_mat);
  cvSet2D(&cv_gl_mat, 0, 3, cvScalar(flp * tra[0]));
  cvSet2D(&cv_gl_mat, 1, 3, cvScalar(flp * tra[1]));
  cvSet2D(&cv_gl_mat, 2, 3, cvScalar(flp * tra[2]));
  cvSet2D(&cv_gl_mat, 3, 3, cvScalar(1));

  cvTranspose(&cv_gl_mat, &cv_gl_mat);
}

void Drawable::SetGLMatTraRod(double* tra, double* rod)
{
  // This is the OpenGL augmentation matrix
  CvMat cv_gl_mat;
  cvInitMatHeader(&cv_gl_mat, 4, 4, CV_64F, gl_mat);
  cvSetIdentity(&cv_gl_mat);

  // Figure out the rotation part
  double rot_mat_data[3][3];
  CvMat rot_mat = cvMat(3, 3, CV_64F, rot_mat_data);
  cvSetIdentity(&rot_mat);
  if (rod != 0)
  {
    CvMat rod_mat;
    cvInitMatHeader(&rod_mat, 3, 1, CV_64F, rod);
    cvRodrigues2(&rod_mat, &rot_mat, 0);
  }

  // Fill in the rotation part
  cvmSet(&cv_gl_mat, 0, 0, cvmGet(&rot_mat, 0, 0));
  cvmSet(&cv_gl_mat, 0, 1, cvmGet(&rot_mat, 0, 1));
  cvmSet(&cv_gl_mat, 0, 2, cvmGet(&rot_mat, 0, 2));
  cvmSet(&cv_gl_mat, 1, 0, cvmGet(&rot_mat, 1, 0));
  cvmSet(&cv_gl_mat, 1, 1, cvmGet(&rot_mat, 1, 1));
  cvmSet(&cv_gl_mat, 1, 2, cvmGet(&rot_mat, 1, 2));
  cvmSet(&cv_gl_mat, 2, 0, cvmGet(&rot_mat, 2, 0));
  cvmSet(&cv_gl_mat, 2, 1, cvmGet(&rot_mat, 2, 1));
  cvmSet(&cv_gl_mat, 2, 2, cvmGet(&rot_mat, 2, 2));

  // Fill in the translation part
  cvSet2D(&cv_gl_mat, 0, 3, cvScalar(tra[0]));
  cvSet2D(&cv_gl_mat, 1, 3, cvScalar(tra[1]));
  cvSet2D(&cv_gl_mat, 2, 3, cvScalar(tra[2]));

  // Transpose into OpenGL presentation order
  cvTranspose(&cv_gl_mat, &cv_gl_mat);
}

Mutex mutex_items;
vector<Drawable*> items;

int cur_button;
float elev = 0.0, azim = 0.0, rad = 0.0;
float panx = 0.0, pany = 0.0;
float jaw = 0.0, jawx, jawy, jawz;

int ar_window;
int vr_window;

float off_x = 0, off_y = 0;

unsigned char* image = 0;

int a_argc;
char** a_argv;
int width;
int height;

Threads threads;

double proj_mat[16];
double modelview_mat[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 };

static void* glut_thread(void* lpThreadParameter)
{
  // InitializeCriticalSection(&critical_section_items);

  glutInit(&a_argc, a_argv);
  glutInitDisplayMode(GLUT_DEPTH | GLUT_RGB | GLUT_DOUBLE);
  glutInitWindowSize(width, height);

  ar_window = glutCreateWindow("AR");
  glutDisplayFunc(DrawAr);
  glutSpecialFunc(KeyCallback);
  glutPositionWindow(0, 0);

  vr_window = glutCreateWindow("VR");
  glutDisplayFunc(DrawVr);
  glutPositionWindow(0, height);

  glEnable(GL_CULL_FACE);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_COLOR_MATERIAL);

  glutMouseFunc(Mouse);
  glutMotionFunc(Motion);

  atexit(Exit);

  glutMainLoop();
  return 0;
}

void GlutViewer::KeyCallback(int key, int x, int y)
{
  switch (key)
  {
    case GLUT_KEY_LEFT:
      off_x -= 1;
      break;
    case GLUT_KEY_RIGHT:
      off_x += 1;
      break;
    case GLUT_KEY_UP:
      off_y -= 1;
      break;
    case GLUT_KEY_DOWN:
      off_y += 1;
      break;
  }
}

double GlutViewer::GetXOffset()
{
  return off_x;
}

double GlutViewer::GetYOffset()
{
  return off_y;
}

void GlutViewer::Start(int argc, char** argv, int w, int h, float r)
{
  a_argc = argc;
  a_argv = argv;
  width = w;
  height = h;
  rad = r;

  threads.create(glut_thread, 0);
}

void GlutViewer::DrawFloor()
{
  glColor3f(0.5, 0.5, 1.0);

  glBegin(GL_LINES);
  for (int i = -20; i <= 20; i += 1)
  {
    glVertex3f((float)i, 0.0f, -20);
    glVertex3f((float)i, 0.0f, 20);
    glVertex3f(-20, 0.0f, (float)i);
    glVertex3f(20, 0.0f, (float)i);
  }
  glEnd();
}

void GlutViewer::Mouse(int button, int state, int x, int y)
{
  cur_button = button;
}

void GlutViewer::DrawAxis(float scale)
{
  glColor3f(1.0, 0.0, 0.0);
  glBegin(GL_LINES);
  glVertex3f(0.0, 0.0, 0.0);
  glVertex3f(scale, 0.0, 0.0);
  glEnd();
  glColor3f(0.0, 1.0, 0.0);
  glBegin(GL_LINES);
  glVertex3f(0.0, 0.0, 0.0);
  glVertex3f(0.0, scale, 0.0);
  glEnd();
  glColor3f(0.0, 0.0, 1.0);
  glBegin(GL_LINES);
  glVertex3f(0.0, 0.0, 0.0);
  glVertex3f(0.0, 0.0, scale);
  glEnd();
}

void GlutViewer::Motion(int x, int y)
{
  static int oldx, oldy;

  int dx = oldx - x;
  int dy = oldy - y;

  switch (cur_button)
  {
    case GLUT_LEFT_BUTTON:
      if (abs(dx) > abs(dy))
      {
        if (dx > 0)
          azim += 3.0;
        else if (dx < 0)
          azim -= 3.0;
      }
      else if (dy > 0)
        elev -= 3.0;
      else if (dy < 0)
        elev += 3.0;
      break;

    case GLUT_MIDDLE_BUTTON:
      if (abs(dx) > abs(dy))
      {
        if (dx > 0)
          panx += 10.5;
        else if (dx < 0)
          panx -= 10.5;
      }
      else if (dy > 0)
        pany -= 10.5;
      else if (dy < 0)
        pany += 10.5;
      break;  //??

    case GLUT_RIGHT_BUTTON:
      if (dy > 0)
        rad += (10.2);
      else if (dy < 0)
        rad -= (10.2);
      break;

    default:
      break;
  }

  oldx = x;
  oldy = y;
}

void GlutViewer::DrawContent()
{
  DrawAxis(100.f);
  Lock lock(&mutex_items);
  for (unsigned i = 0; i < items.size(); ++i)
  {
    items[i]->Draw();
  }
}

void GlutViewer::DrawVr()
{
  glutSetWindow(vr_window);
  glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
  glClearColor(0.5, 0.2, 0.2, 1.0);

  // glViewport(0, 0, w, h);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  // gluPerspective(70, 1, 0.001, 5000);

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  glTranslatef(panx, pany, -rad);
  glRotatef(-elev, 1.0, 0.0, 0.0);
  glRotatef(azim, 0.0, 1.0, 0.0);

  float pos[4] = { 50, 0, 50 };
  glLightfv(GL_LIGHT0, GL_POSITION, pos);

  DrawContent();

  // glFlush();
  glutSwapBuffers();
  glutPostRedisplay();
}

void GlutViewer::DrawAr()
{
  glutSetWindow(ar_window);
  glClearColor(0.2, 0.5, 0.2, 1.0);
  glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);

  DrawVideo();

  glMatrixMode(GL_PROJECTION);
  glLoadMatrixd(proj_mat);

  glMatrixMode(GL_MODELVIEW);
  glLoadMatrixd(modelview_mat);

  DrawContent();

  glutSwapBuffers();
  glutPostRedisplay();
}

void GlutViewer::SetGlProjectionMatrix(double p[16])
{
  memcpy(proj_mat, p, sizeof(double) * 16);
}

void GlutViewer::SetGlModelviewMatrix(double p[16])
{
  memcpy(modelview_mat, p, sizeof(double) * 16);
}

void GlutViewer::Reshape(int w, int h)
{
  h = (h == 0 ? 1 : h);
  // glViewport(0, 0, w, h);

  // glMatrixMode(GL_PROJECTION);

  // glLoadIdentity();
  // glMultMatrixd(projmat);

  // glMatrixMode(GL_MODELVIEW);

  // glutPostRedisplay();
}

void GlutViewer::Exit()
{
  // DeleteCriticalSection(&critical_section_items);
}

void GlutViewer::DrawableClear()
{
  Lock lock(&mutex_items);
  items.clear();
}

void GlutViewer::DrawableAdd(Drawable* item)
{
  Lock lock(&mutex_items);
  items.push_back(item);
}

// TODO ensure that threading doesn't cause any problems...
void GlutViewer::SetVideo(const IplImage* _image)
{
  image = (unsigned char*)_image->imageData;
}

void GlutViewer::DrawVideo()
{
  if (!image)
    return;

  glDepthMask(GL_FALSE);
  glDisable(GL_DEPTH_TEST);

  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);

  glColor3f(1, 1, 1);
  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 512, 512, 0, GL_BGR_EXT,
               GL_UNSIGNED_BYTE, image);

  glEnable(GL_TEXTURE_2D);

  glMatrixMode(GL_PROJECTION);
  glPushMatrix();
  glLoadIdentity();
  glOrtho(0, 1, 0, 1, 0, 1);

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  glBegin(GL_QUADS);
  glTexCoord2d(0.0, 1.0);
  glVertex2d(0.0, 0.0);
  glTexCoord2d(1.0, 1.0);
  glVertex2d(1.0, 0.0);
  glTexCoord2d(1.0, 0.0);
  glVertex2d(1.0, 1.0);
  glTexCoord2d(0.0, 0.0);
  glVertex2d(0.0, 1.0);
  glEnd();

  glDisable(GL_TEXTURE_2D);
  glDepthMask(GL_TRUE);
  glEnable(GL_DEPTH_TEST);

  glMatrixMode(GL_PROJECTION);
  glPopMatrix();
}

/*
void GlutViewer::Init(int argc, char** argv, int w, int h)
{
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DEPTH | GLUT_RGB | GLUT_DOUBLE);
  glutInitWindowSize(w, h);

  ar_window = glutCreateWindow("AR");
  glutDisplayFunc(DrawAr);
  //glutReshapeFunc(Reshape);

  vr_window = glutCreateWindow("VR");
  glutDisplayFunc(DrawVr);
  //glutReshapeFunc(Reshape);

  glEnable(GL_CULL_FACE);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_COLOR_MATERIAL);

  glutMouseFunc(Mouse);
  glutMotionFunc(Motion);

  atexit(Exit);
}
*/
